/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.templates.subsystems.NewPIDLifterSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.SwerveSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.ShooterSubsystem;

/**
 *
 * @author matt
 */
public class Calibrate extends CommandBase {

    private boolean m_isRunning = false;
    private static NewPIDLifterSubsystem lifter;
    private int adjustment;
    private static ShooterSubsystem VexMotor;
    private static boolean calLifter = false;
    private static boolean calSwerve = false;
    private static boolean calArm = false;
    private static boolean calShooterVex = false;
    private static boolean calShooterJag = false;
    private static double currentLifterPositionCommand;
    private static double currentSwervePositionCommand;

    public Calibrate(int theAdjustment) {
        // Use requires() here to declare subsystem dependencies
//        requires(leftForeLifterSubsystem);
//        requires(leftRearLifterSubsystem);
//        requires(rightForeLifterSubsystem);
//        requires(rightRearLifterSubsystem); 
//        requires(armSubsystem); 
//        requires(swerveSubsystem);  

        lifter = leftForeLifterSubsystem;
        VexMotor = shooterSubsystem;
        adjustment = theAdjustment;


        System.out.println("CalibrateLifter constructed " + lifter.getName() + " adjust " + adjustment);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        m_isRunning = true;
        // command drives to slow to the point where we can deploy
        leftDriveSubsystem.drive(0);
        rightDriveSubsystem.drive(0);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {

        // check to see if the lifter to cal has changed
        if (oi.getCalibrationController().getButtonY()) {         // LF
            System.out.println("Calibrate LF");
            lifter = leftForeLifterSubsystem;
            currentLifterPositionCommand = lifter.getPosition();
            calLifter = true;
            calSwerve = false;
            calArm = false;
            calShooterVex = false;
            calShooterJag =false;
        } else if (oi.getCalibrationController().getButtonX()) { // LR
            System.out.println("Calibrate LR");
            lifter = leftRearLifterSubsystem;
            currentLifterPositionCommand = lifter.getPosition();
            calLifter = true;
            calSwerve = false;
            calArm = false;
            calShooterVex = false;
            calShooterJag =false;
        } else if (oi.getCalibrationController().getButtonB()) { // RF
            System.out.println("Calibrate RF");
            lifter = rightForeLifterSubsystem;
            currentLifterPositionCommand = lifter.getPosition();
            calLifter = true;
            calSwerve = false;
            calArm = false;
            calShooterVex = false;
            calShooterJag =false;
        } else if (oi.getCalibrationController().getButtonA()) { // RR
            System.out.println("Calibrate RR");
            lifter = rightRearLifterSubsystem;
            currentLifterPositionCommand = lifter.getPosition();
            calLifter = true;
            calSwerve = false;
            calArm = false;
            calShooterVex = false;
            calShooterJag =false;
        } else if (oi.getCalibrationController().getButtonStart()) { // Arm
            System.out.println("Calibrate Arm");
            calLifter = false;
            calSwerve = false;
            calArm = true;
            calShooterVex = false;
            calShooterJag =false;
        } else if (oi.getCalibrationController().getButtonBack()) { // Swerve
            System.out.println("Calibrate Swerve");
            currentSwervePositionCommand = swerveSubsystem.getPosition();
            calLifter = false;
            calSwerve = true;
            calArm = false;
            calShooterVex = false;
            calShooterJag =false;
        } 
//        else if (oi.getCalibrationController().getButtonGameUp()) { // Shooter Vex
//            System.out.println("Calibrate Shooter Vex");
//            calLifter = false;
//            calSwerve = false;
//            calArm = false;
//            calShooterVex = true;
//            calShooterJag =false;
//        } else if (oi.getCalibrationController().getButtonGameUp()) { // Shooter Jag
//            System.out.println("Calibrate Shooter Jag");
//            calLifter = false;
//            calSwerve = false;
//            calArm = false;
//            calShooterVex = false;
//            calShooterJag = true;
//        }

        if (calLifter) {
            currentLifterPositionCommand += adjustment;
            lifter.setLifterPosition((int) currentLifterPositionCommand);
            System.out.println("Lifter - " + lifter.getName() + " @ " + lifter.getPosition());
        } else if (calArm) {
            armSubsystem.adjustArm(0.001 * adjustment);
            System.out.println("ARM - @ " + armSubsystem.getPosition());
        } else if (calSwerve) {
            currentSwervePositionCommand += 0.001 * adjustment;
            swerveSubsystem.setPosition(currentSwervePositionCommand);
            System.out.println("Swerve - @ " + swerveSubsystem.getPosition());
        } 
//        else if (calShooterVex) {
//            if (-10 == adjustment) {
//                System.out.println("Shooter Vex Motor is ingesting.");
//                VexMotor.VexOnlyIngest();
//            } else if (10 == adjustment) {
//                System.out.println("Shooter Vex Motor is shooting.");
//                VexMotor.shoot();
//            } else if ((-1 == adjustment) || (1 == adjustment)) {
//                System.out.println("Shooter Vex Motor is stopped.");
//                VexMotor.VexOnlyStop();
//            }
//        }
//        else if (calShooterJag) {
//            if(-10 == adjustment) {
//                System.out.println("Shooter Jag Motor is ingesting");
//                VexMotor.JagOnlyIngest();
//            } else if (10 == adjustment) {
//                System.out.println("Shooter Jag Motor is shooting");
//                VexMotor.spinUpShooter();
//            } else if ((-1 == adjustment) || (1 == adjustment)) {
//                System.out.println("Shooter Jag Motor is stopped.");
//            }
//        }
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return m_isRunning;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }

    private void requires(ArmSubsystem armSubsystem) {
    }
}
